#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <boost/filesystem.hpp>

// 保存点云的目录
std::string output_dir = "/home/cxr/wujie_ws/src/pcd_pkg/pcd";

// 点云保存回调函数
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) {
    // 将 ROS 的 PointCloud2 消息转换为 PCL 点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *cloud);

    // 生成保存文件名
    std::string file_name = output_dir + "/point_cloud.pcd";

    // 保存为 PCD 文件
    if (pcl::io::savePCDFileBinary(file_name, *cloud) == 0) {
        ROS_INFO("Saved point cloud to %s", file_name.c_str());
    } else {
        ROS_ERROR("Failed to save point cloud to %s", file_name.c_str());
    }
}

int main(int argc, char** argv) {
    // 初始化 ROS 节点
    ros::init(argc, argv, "point_cloud_saver");
    ros::NodeHandle nh;

    // 检查保存目录是否存在，如果不存在则创建
    if (!boost::filesystem::exists(output_dir)) {
        boost::filesystem::create_directory(output_dir);
    }

    // 订阅 /laser_map 话题
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/new_cloud", 1, pointCloudCallback);

    // 保持节点运行
    ros::spin();

    return 0;
}